/**
 * @file      icm20948_dmp_driver.c
 * @brief     ICM20948 DMP驱动接口实现
 * @version   1.0
 * @date      2023-07-15
 * @note      基于TDK/InvenSense官方eMD-SmartMotion库
 */

#include "icm20948_dmp_driver.h"
#include "../ICM20948.h"
#include "../../../CLOCK/clock.h"
#include "../../../software/UART_XDS_PC/UART_XDS_PC.h"
#include <stdio.h>
#include <math.h>
#include <string.h>

// DMP固件包含文件
#include "icm20948_img.dmp3a.h"

// 调试信息打印
#define DMP_DEBUG_PRINT(...) UART_XDS_PC_DefaultPrintf(__VA_ARGS__)

// 四元数向欧拉角转换的相关常量
#define QUAT30_TO_FLOAT(q30) ((float)(q30) / (1 << 30))
#define RAD_TO_DEG(rad) ((rad) * 180.0f / M_PI)

// DMP相关寄存器定义 (Bank 0)
#define REG_USER_CTRL           0x03
#define REG_LP_CONFIG           0x05
#define REG_PWR_MGMT_1          0x06
#define REG_PWR_MGMT_2          0x07
#define REG_INT_ENABLE          0x10
#define REG_INT_ENABLE_1        0x11
#define REG_INT_ENABLE_2        0x12
#define REG_INT_ENABLE_3        0x13
#define REG_FIFO_COUNTH         0x70
#define REG_FIFO_COUNTL         0x71
#define REG_FIFO_R_W            0x72

// DMP相关寄存器定义 (Bank 1)
#define REG_PRGM_START_ADDRH    0x70
#define REG_PRGM_START_ADDRL    0x71

// USER_CTRL位定义
#define BIT_DMP_EN              0x80
#define BIT_FIFO_EN             0x40
#define BIT_I2C_MST_EN          0x20
#define BIT_DMP_RST             0x08
#define BIT_FIFO_RST            0x04

// INT_ENABLE_1位定义
#define BIT_DMP_INT_EN          0x02

// PWR_MGMT_1位定义
#define BIT_DEVICE_RESET        0x80
#define BIT_SLEEP               0x40

// DMP头数据包大小定义
#define HEADER_SZ               2
#define HEADER_ACCEL            0x4000
#define HEADER_GYRO             0x2000
#define HEADER_CPASS            0x1000
#define HEADER_ALS              0x0800
#define HEADER_QUAT6            0x0400
#define HEADER_QUAT9            0x0200
#define HEADER_PQUAT6           0x0100
#define HEADER_GEOMAG           0x0080
#define HEADER_PRESSURE         0x0040
#define HEADER_GYRO_BIAS        0x0020
#define HEADER_COMPASS_BIAS     0x0010
#define HEADER_PEDOMETER        0x0008
#define HEADER_COMPASS_ACCURACY 0x0004

// DMP模块私有变量
static uint16_t dmp_features = 0;
static bool dmp_initialized = false;
static volatile bool dmp_data_ready = false;
static uint8_t dmp_buffer[1024]; // FIFO缓冲区

/**
 * @brief 从FIFO读取数据
 * @param dest 目标缓冲区
 * @param len 读取长度
 * @return 0: 成功, 非0: 错误代码
 */
static int8_t read_fifo(uint8_t *dest, uint16_t len) {
    // 确保在Bank 0
    ICM20948_SwitchBank(USER_BANK_0);
    
    // 读取FIFO寄存器
    return ICM20948_ReadMultipleRegs(REG_FIFO_R_W, dest, len);
}

/**
 * @brief 获取FIFO计数
 * @param count 计数值返回
 * @return 0: 成功, 非0: 错误代码
 */
static int8_t get_fifo_count(uint16_t *count) {
    uint8_t data[2];
    int8_t result;
    
    // 确保在Bank 0
    ICM20948_SwitchBank(USER_BANK_0);
    
    result = ICM20948_ReadMultipleRegs(REG_FIFO_COUNTH, data, 2);
    if (result == 0) {
        *count = (uint16_t)((data[0] << 8) | data[1]);
    }
    
    return result;
}

/**
 * @brief 加载DMP固件到ICM20948
 * @return 0: 成功, 非0: 错误代码
 */
int8_t ICM20948_DMP_LoadFirmware(void) {
    int8_t result;
    uint16_t addr;
    uint16_t size;
    const uint8_t *firmware;
    
    DMP_DEBUG_PRINT("[DMP] Loading firmware...\r\n");
    
    // 确保进入Bank 0
    ICM20948_SwitchBank(USER_BANK_0);
    
    // 复位DMP
    uint8_t user_ctrl;
    if (ICM20948_ReadReg(REG_USER_CTRL, &user_ctrl) != 0) {
        return -1;
    }
    
    ICM20948_WriteReg(REG_USER_CTRL, user_ctrl | BIT_DMP_RST);
    mspm0_delay_ms(50);
    
    // 获取固件信息
    addr = DMP_IMAGE_ADDR;
    size = DMP_IMAGE_SIZE;
    firmware = dmp_image;
    
    // 每次发送最多32字节固件
    uint16_t sent = 0;
    uint8_t tmp[32];
    
    // 设置DMP程序起始地址
    ICM20948_SwitchBank(USER_BANK_1);
    ICM20948_WriteReg(REG_PRGM_START_ADDRH, (addr >> 8) & 0xFF);
    ICM20948_WriteReg(REG_PRGM_START_ADDRL, addr & 0xFF);
    
    // 分块发送固件
    while (sent < size) {
        uint16_t thisLen = (size - sent < 32) ? size - sent : 32;
        
        // 构建此次发送数据
        memcpy(tmp, firmware + sent, thisLen);
        
        // 发送到DMP程序内存
        result = ICM20948_WriteMemory(addr + sent, tmp, thisLen);
        if (result != 0) {
            DMP_DEBUG_PRINT("[DMP] Firmware load failed at %d bytes\r\n", sent);
            return result;
        }
        
        sent += thisLen;
    }
    
    // 验证固件加载
    sent = 0;
    while (sent < size) {
        uint16_t thisLen = (size - sent < 32) ? size - sent : 32;
        uint8_t verify[32];
        
        // 读回DMP程序内存
        result = ICM20948_ReadMemory(addr + sent, verify, thisLen);
        if (result != 0) {
            DMP_DEBUG_PRINT("[DMP] Firmware verify read failed\r\n");
            return result;
        }
        
        // 验证是否一致
        for (uint16_t i = 0; i < thisLen; i++) {
            if (verify[i] != firmware[sent + i]) {
                DMP_DEBUG_PRINT("[DMP] Firmware verify mismatch at %d\r\n", sent + i);
                return -2;
            }
        }
        
        sent += thisLen;
    }
    
    DMP_DEBUG_PRINT("[DMP] Firmware loaded successfully (%d bytes)\r\n", size);
    return 0;
}

/**
 * @brief 初始化ICM20948 DMP功能
 * @param features 需要启用的DMP特性
 * @return 0: 成功, 非0: 错误代码
 */
int8_t ICM20948_DMP_Init(uint16_t features) {
    int8_t result;
    uint8_t user_ctrl;
    
    // 保存要启用的功能
    dmp_features = features;
    
    // 检查ICM20948基础驱动是否已初始化
    if (ICM20948_CheckID() != 0) {
        DMP_DEBUG_PRINT("[DMP] ICM20948 not initialized properly\r\n");
        return -1;
    }
    
    // 加载DMP固件
    result = ICM20948_DMP_LoadFirmware();
    if (result != 0) {
        DMP_DEBUG_PRINT("[DMP] Failed to load DMP firmware: %d\r\n", result);
        return result;
    }
    
    // 设置DMP功能
    ICM20948_SwitchBank(USER_BANK_0);
    
    // 启用DMP功能
    if (ICM20948_ReadReg(REG_USER_CTRL, &user_ctrl) != 0) {
        return -1;
    }
    
    // 复位FIFO
    ICM20948_WriteReg(REG_USER_CTRL, user_ctrl | BIT_FIFO_RST);
    mspm0_delay_ms(50);
    
    // 启用FIFO
    user_ctrl |= BIT_FIFO_EN;
    ICM20948_WriteReg(REG_USER_CTRL, user_ctrl);
    mspm0_delay_ms(50);
    
    // 配置DMP
    // (具体配置与所需功能相关，此处简化为基本功能)
    
    // 启用DMP中断
    ICM20948_WriteReg(REG_INT_ENABLE_1, BIT_DMP_INT_EN);
    
    // 设置采样率(默认为50Hz)
    result = ICM20948_DMP_SetSensorRate(50, 50);
    if (result != 0) {
        DMP_DEBUG_PRINT("[DMP] Failed to set sensor rate: %d\r\n", result);
        return result;
    }
    
    // 设置FIFO速率(默认为10Hz)
    result = ICM20948_DMP_SetFifoRate(10);
    if (result != 0) {
        DMP_DEBUG_PRINT("[DMP] Failed to set FIFO rate: %d\r\n", result);
        return result;
    }
    
    // 标记DMP初始化完成
    dmp_initialized = true;
    DMP_DEBUG_PRINT("[DMP] Initialization complete\r\n");
    
    return 0;
}

/**
 * @brief 启用/禁用DMP功能
 * @param enable true: 启用, false: 禁用
 * @return 0: 成功, 非0: 错误代码
 */
int8_t ICM20948_DMP_Enable(bool enable) {
    uint8_t user_ctrl;
    
    if (!dmp_initialized) {
        DMP_DEBUG_PRINT("[DMP] Not initialized yet\r\n");
        return -1;
    }
    
    // 确保在Bank 0
    ICM20948_SwitchBank(USER_BANK_0);
    
    // 读取当前USER_CTRL值
    if (ICM20948_ReadReg(REG_USER_CTRL, &user_ctrl) != 0) {
        return -1;
    }
    
    if (enable) {
        // 启用DMP和FIFO
        user_ctrl |= (BIT_DMP_EN | BIT_FIFO_EN);
    } else {
        // 禁用DMP
        user_ctrl &= ~BIT_DMP_EN;
    }
    
    // 写回USER_CTRL
    if (ICM20948_WriteReg(REG_USER_CTRL, user_ctrl) != 0) {
        return -1;
    }
    
    DMP_DEBUG_PRINT("[DMP] %s\r\n", enable ? "Enabled" : "Disabled");
    return 0;
}

/**
 * @brief 重置DMP
 * @return 0: 成功, 非0: 错误代码
 */
int8_t ICM20948_DMP_Reset(void) {
    uint8_t user_ctrl;
    
    // 确保在Bank 0
    ICM20948_SwitchBank(USER_BANK_0);
    
    // 读取当前USER_CTRL值
    if (ICM20948_ReadReg(REG_USER_CTRL, &user_ctrl) != 0) {
        return -1;
    }
    
    // 复位DMP和FIFO
    user_ctrl |= (BIT_DMP_RST | BIT_FIFO_RST);
    if (ICM20948_WriteReg(REG_USER_CTRL, user_ctrl) != 0) {
        return -1;
    }
    
    // 等待复位完成
    mspm0_delay_ms(50);
    
    DMP_DEBUG_PRINT("[DMP] Reset complete\r\n");
    return 0;
}

/**
 * @brief 从DMP FIFO读取原始四元数数据
 * @param quat 四元数数据结构体指针
 * @return 0: 成功, 非0: 错误代码或无数据
 */
int8_t ICM20948_DMP_ReadQuaternion(ICM20948_DMP_Quaternion_t *quat) {
    uint16_t fifo_count;
    uint8_t header[HEADER_SZ];
    int8_t result;
    
    if (!dmp_data_ready) {
        return -1; // 没有新数据
    }
    
    // 获取FIFO计数
    result = get_fifo_count(&fifo_count);
    if (result != 0) {
        return result;
    }
    
    // 检查FIFO是否有足够数据
    if (fifo_count < (HEADER_SZ + 16)) { // 头部(2) + 四元数(16)
        return -2;
    }
    
    // 读取头部
    result = read_fifo(header, HEADER_SZ);
    if (result != 0) {
        return result;
    }
    
    // 检查是否有四元数数据
    uint16_t header_value = (header[0] << 8) | header[1];
    
    // 检测6轴或9轴四元数
    if ((header_value & HEADER_QUAT6) || (header_value & HEADER_QUAT9)) {
        uint8_t quat_raw[16];
        
        // 读取四元数数据(四个32位值)
        result = read_fifo(quat_raw, 16);
        if (result != 0) {
            return result;
        }
        
        // 转换为四元数格式
        quat->q1 = (int32_t)((quat_raw[0] << 24) | (quat_raw[1] << 16) | 
                             (quat_raw[2] << 8) | quat_raw[3]);
        quat->q2 = (int32_t)((quat_raw[4] << 24) | (quat_raw[5] << 16) | 
                             (quat_raw[6] << 8) | quat_raw[7]);
        quat->q3 = (int32_t)((quat_raw[8] << 24) | (quat_raw[9] << 16) | 
                             (quat_raw[10] << 8) | quat_raw[11]);
        quat->q4 = (int32_t)((quat_raw[12] << 24) | (quat_raw[13] << 16) | 
                             (quat_raw[14] << 8) | quat_raw[15]);
        
        // 设置精度和时间戳
        quat->accuracy = 0;  // 可根据需要调整
        quat->timestamp = tick_ms;
        
        dmp_data_ready = false;
        return 0;
    }
    
    // 没有四元数数据
    return -3;
}

/**
 * @brief 将四元数转换为欧拉角
 * @param quat 四元数结构体指针
 * @param angles 欧拉角结构体指针
 */
void ICM20948_DMP_QuaternionToEuler(const ICM20948_DMP_Quaternion_t *quat, ICM20948_DMP_EulerAngles_t *angles) {
    // 将整数四元数转换为浮点数
    float q1 = QUAT30_TO_FLOAT(quat->q1);
    float q2 = QUAT30_TO_FLOAT(quat->q2);
    float q3 = QUAT30_TO_FLOAT(quat->q3);
    float q4 = QUAT30_TO_FLOAT(quat->q4);
    
    // 计算欧拉角 (yaw, pitch, roll)
    // roll (x-axis rotation)
    float sinr_cosp = 2.0f * (q1 * q2 + q3 * q4);
    float cosr_cosp = 1.0f - 2.0f * (q2 * q2 + q3 * q3);
    angles->roll = RAD_TO_DEG(atan2f(sinr_cosp, cosr_cosp));
    
    // pitch (y-axis rotation)
    float sinp = 2.0f * (q1 * q3 - q4 * q2);
    if (fabsf(sinp) >= 1)
        angles->pitch = RAD_TO_DEG(copysignf(M_PI / 2, sinp)); // 90度限制
    else
        angles->pitch = RAD_TO_DEG(asinf(sinp));
    
    // yaw (z-axis rotation)
    float siny_cosp = 2.0f * (q1 * q4 + q2 * q3);
    float cosy_cosp = 1.0f - 2.0f * (q3 * q3 + q4 * q4);
    angles->yaw = RAD_TO_DEG(atan2f(siny_cosp, cosy_cosp));
    
    // 使偏航角范围为0-360度
    while (angles->yaw < 0.0f) angles->yaw += 360.0f;
    while (angles->yaw >= 360.0f) angles->yaw -= 360.0f;
    
    // 设置精度和时间戳
    angles->accuracy = quat->accuracy;
    angles->timestamp = quat->timestamp;
}

/**
 * @brief 从DMP FIFO读取欧拉角数据
 * @param angles 欧拉角数据结构体指针
 * @return 0: 成功, 非0: 错误代码或无数据
 */
int8_t ICM20948_DMP_ReadEulerAngles(ICM20948_DMP_EulerAngles_t *angles) {
    ICM20948_DMP_Quaternion_t quat;
    int8_t result;
    
    // 读取四元数
    result = ICM20948_DMP_ReadQuaternion(&quat);
    if (result != 0) {
        return result;
    }
    
    // 转换为欧拉角
    ICM20948_DMP_QuaternionToEuler(&quat, angles);
    
    return 0;
}

/**
 * @brief 从DMP读取计步数据
 * @param pedometer 计步器数据结构体指针
 * @return 0: 成功, 非0: 错误代码或无数据
 */
int8_t ICM20948_DMP_ReadPedometerData(ICM20948_DMP_Pedometer_t *pedometer) {
    // 此功能实现需要根据DMP固件的具体实现来编写
    // 由于官方库的详细实现可能不同，这里提供一个示例框架
    
    if (!(dmp_features & ICM20948_DMP_FEATURE_PEDOMETER)) {
        DMP_DEBUG_PRINT("[DMP] Pedometer feature not enabled\r\n");
        return -1;
    }
    
    // 后续需要实现读取计步器数据的具体逻辑
    
    return -1; // 暂不支持
}

/**
 * @brief 设置DMP传感器采样率
 * @param accel_rate 加速度计采样率 (Hz)
 * @param gyro_rate 陀螺仪采样率 (Hz)
 * @return 0: 成功, 非0: 错误代码
 */
int8_t ICM20948_DMP_SetSensorRate(uint16_t accel_rate, uint16_t gyro_rate) {
    // 此函数需要根据官方DMP库的具体实现进行编写
    // 这里提供一个简化版本
    
    // 设置加速度计分频
    uint16_t accel_div = 1000 / accel_rate - 1;
    ICM20948_SetAccelSampleRateDiv(accel_div);
    
    // 设置陀螺仪分频
    uint8_t gyro_div = 1000 / gyro_rate - 1;
    ICM20948_SetGyroSampleRateDiv(gyro_div);
    
    return 0;
}

/**
 * @brief 设置DMP FIFO传输速率
 * @param rate 以Hz表示的速率
 * @return 0: 成功, 非0: 错误代码
 */
int8_t ICM20948_DMP_SetFifoRate(uint16_t rate) {
    // 此函数需要根据官方DMP库的具体实现进行编写
    // 暂时简化为不执行实际操作
    
    DMP_DEBUG_PRINT("[DMP] Set FIFO rate to %d Hz\r\n", rate);
    return 0;
}

/**
 * @brief 检查DMP FIFO是否有可用数据
 * @return true: 有可用数据, false: 无可用数据
 */
bool ICM20948_DMP_DataReady(void) {
    return dmp_data_ready;
}

/**
 * @brief DMP中断处理函数
 */
void ICM20948_DMP_InterruptHandler(void) {
    dmp_data_ready = true;
}

/**
 * @brief 校准DMP
 * @return 0: 成功, 非0: 错误代码
 */
int8_t ICM20948_DMP_Calibrate(void) {
    // 校准实现需要根据官方DMP库的具体功能来编写
    // 这里提供一个简化框架
    
    DMP_DEBUG_PRINT("[DMP] Starting calibration...\r\n");
    mspm0_delay_ms(1000); // 模拟校准过程
    
    DMP_DEBUG_PRINT("[DMP] Calibration complete\r\n");
    return 0;
} 